# -*- coding: utf-8 -*-
"""
二维云台控制器 (基于串口通信)
说明: 这是一个通用接口，后续需根据实际舵机协议适配。
"""
import serial
import time
import threading
from src.config import PAN_TILT_CONFIG


class PanTiltController:
    def __init__(self):
        self.port = PAN_TILT_CONFIG['port']
        self.baudrate = PAN_TILT_CONFIG['baudrate']
        self.ser = None
        self.lock = threading.Lock()  # 确保串口操作线程安全

        # 当前舵机角度记录
        self.current_pan_angle = 90  # 水平舵机初始角度 (中间位置)
        self.current_tilt_angle = 90  # 垂直舵机初始角度 (中间位置)

        self._connect()

    def _connect(self):
        """初始化并连接串口"""
        try:
            # 如果是虚拟串口模式（调试用）
            if self.port == 'virtual_echo_port':
                print("虚拟串口模式已启用，用于调试回显。")
                return

            self.ser = serial.Serial(
                port=self.port,
                baudrate=self.baudrate,
                bytesize=8,
                stopbits=1,
                parity='N',
                timeout=1  # 读超时1秒
            )
            print(f"串口 {self.port} 已连接，波特率 {self.baudrate}")

        except Exception as e:
            print(f"串口连接失败: {e}")
            print("将使用虚拟回显模式进行调试。")

    def _send_packet(self, packet_data):
        """
        核心方法：发送数据包到串口，并处理回显（如果启用）
        参数:
            packet_data: 要发送的字节数据
        返回:
            str: 描述发送（和接收）的状态信息
        """
        # 虚拟串口模式
        if self.port == 'virtual_echo_port' or self.ser is None:
            simulated_response = f"Echo: {packet_data.hex()}"
            print(f"[VIRTUAL SENT] {packet_data.hex()}")
            print(f"[VIRTUAL RECEIVED] {simulated_response}")
            return f"Virtual mode: {simulated_response}"

        # 物理串口模式
        if self.ser and self.ser.is_open:
            try:
                with self.lock:
                    self.ser.write(packet_data)
                    print(f"[SENT] {packet_data.hex()}")  # 打印发送的原始字节

                    # 可选：读取舵机可能的响应（非阻塞方式简短读取）
                    time.sleep(0.1)  # 短暂等待响应
                    response = self.ser.read_all()
                    if response:
                        print(f"[RECEIVED] {response.hex()}")
                        return f"Data sent and received: {response.hex()}"
                    return "Data sent, no response."

            except Exception as e:
                return f"Error during serial communication: {e}"
        else:
            return "Serial port not available"

    def set_angle(self, servo_id, angle):
        """
        设置指定舵机的角度 (核心控制方法)
        参数:
            servo_id: 舵机ID (例如, 1为水平，2为垂直)
            angle: 目标角度 (度)。注意：某些舵机可能使用脉宽单位或特定角度范围。
        """
        # 角度安全范围检查
        if not (0 <= angle <= 180):  # 假设舵机最大180度
            print(f"错误: 角度 {angle} 超出安全范围(0-180)!")
            return

        # 更新当前角度记录
        if servo_id == PAN_TILT_CONFIG['servo_horz_id']:
            self.current_pan_angle = angle
        elif servo_id == PAN_TILT_CONFIG['servo_vert_id']:
            self.current_tilt_angle = angle

        # ！重要提示：此处需要根据实际舵机协议构建数据包
        # 以下是基于假设协议格式的占位符，你必须替换为实际协议
        packet = self._build_command_packet(servo_id, angle)
        result = self._send_packet(packet)
        print(f"Set servo {servo_id} to {angle}°: {result}")

    def _build_command_packet(self, servo_id, angle):
        """
        根据具体舵机协议构建数据包 (此处为占位符，需替换)
        参数:
            servo_id: 舵机ID
            angle: 目标角度
        返回:
            bytes: 符合协议的命令数据包

        !!! ！！！ 注意 ！！！ ！！！
        这里的数据包格式是占位符，你必须根据你的云台舵机实际通信协议进行修改！
        请查阅你的云台舵机手册，并修改此函数。
        """

        # 示例1：模拟字符串协议 (例如某些舵机使用字符串命令)
        # command_str = f"#{servo_id}P{1500 + angle * (500/90):.0f}T100\n"  # 假设转换公式
        # return command_str.encode()

        # 示例2：模拟二进制协议 (例如LX-16A舵机)
        # 假设协议帧结构: 0x55 0x55 | ID | 数据长度 | 命令字 | 参数(低8位) | 参数(高8位) | 校验和
        # 假设角度转换为0-1000范围（根据舵机分辨率）
        converted_angle = int(angle * (1000 / 180))  # 转换为舵机内部位置值
        pos_lo = converted_angle & 0xFF  # 位置低8位
        pos_hi = (converted_angle >> 8) & 0xFF  # 位置高8位

        # 假设命令字 0x01 为位置控制
        cmd = 0x01
        length = 0x07  # 数据长度（固定或计算）

        # 构建数据包
        packet = bytearray()
        packet.extend([0x55, 0x55])  # 帧头
        packet.append(servo_id)  # 舵机ID
        packet.append(length)  # 数据长度
        packet.append(cmd)  # 命令
        packet.append(pos_lo)  # 参数低字节
        packet.append(pos_hi)  # 参数高字节

        # 校验和计算 (例如，对ID到参数的所有字节求和，取反)
        checksum = ~(server_id + length + cmd + pos_lo + pos_hi) & 0xFF
        packet.append(checksum)

        return bytes(packet)

    def move_relative(self, pan_delta, tilt_delta):
        """
        相对移动云台
        参数:
            pan_delta: 水平方向角度变化量
            tilt_delta: 垂直方向角度变化量
        """
        new_pan_angle = self.current_pan_angle + pan_delta
        new_tilt_angle = self.current_tilt_angle + tilt_delta

        # 限制角度在安全范围内
        new_pan_angle = max(0, min(PAN_TILT_CONFIG['max_pan_angle'], new_pan_angle))
        new_tilt_angle = max(0, min(PAN_TILT_CONFIG['max_tilt_angle'], new_tilt_angle))

        # 设置新角度
        self.set_angle(PAN_TILT_CONFIG['servo_horz_id'], new_pan_angle)
        self.set_angle(PAN_TILT_CONFIG['servo_vert_id'], new_tilt_angle)

        return new_pan_angle, new_tilt_angle

    def stop(self):
        """停止所有舵机（释放扭矩或停止信号）"""
        # 示例：发送停止命令（需要根据实际协议实现）
        # 例如，对于连续旋转舵机，发送中位值；或者发送扭矩释放命令
        print("所有舵机已停止（扭矩释放或停止信号已发送）。")

    def close(self):
        """关闭串口连接"""
        if self.ser and self.ser.is_open:
            self.ser.close()
            print("串口连接已关闭。")